#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <boost/thread.hpp>
#include <ar_track_alvar_msgs/AlvarMarkers.h>
#include "tf/transform_datatypes.h"

#include "fox_msgs/track.h"
#include "fox_msgs/ar_pose.h"
#include <iostream>
using namespace std;


//宏定义参数
#define X_GOAL (0)
#define Z_GOAL (0.20)
#define THETA_GOAL (0)
// velocity control
#define P_THETA (0.7)
#define P_Y (0.4)
#define P_X (0.8)

#define I_THETA (0)
#define I_Y (0.0)
#define I_X (0.0)

#define D_THETA (0.06)
#define D_Y (0.2)
#define D_X (0.1)

#define ADJUST_TIMES (1)
#define REPEAT_TIMES (2)
//pid计算类
class PID_Controller
{
public:
    PID_Controller(double p, double i, double d):m_p(p), m_i(i), m_d(d)
    {
        m_last_err = 0;
        m_i_err = 0;
        m_d_err = 0;
    }
    ~PID_Controller() {}
    double PID(double err)
    {
        m_i_err += m_last_err;
        m_d_err = err - m_last_err;
        double k = m_p*err + m_i*m_i_err + m_d*m_d_err;
        m_last_err = err;
        if(fabs(k) < 0.01) k = fabs(k) / k * 0.01;
        if(fabs(k) > 0.08 ) k = fabs(k) / k * 0.08;
        return k;
    }
private:
    double m_p;
    double m_i;
    double m_d;
    double m_last_err;
    double m_i_err;
    double m_d_err;
};

class PoseAdj
{
public:
    PoseAdj();
    ~PoseAdj();
    int flag, tmp;

    void pose_callback(const ar_track_alvar_msgs::AlvarMarkers& pose_msgs);

    bool TrackCallback(fox_msgs::track::Request &req, 
                        fox_msgs::track::Response &res);
private:
    ros::NodeHandle n;
    ros::Subscriber sub;
    ros::ServiceServer service;
    ros::ServiceServer Track_Server;
    ros::Publisher pub;
    fox_msgs::ar_pose curr_pose;
    int track;
    
};
//类PoseAdj构造函数
PoseAdj::PoseAdj()
{
    track = 1024;
    //接收话题ar_pose
	sub  = n.subscribe("ar_pose_marker", 10, &PoseAdj::pose_callback,this);
    //提供服务/pose_adjust/adjust_service
    Track_Server = n.advertiseService("Track", &PoseAdj::TrackCallback,this);

    //发布话题cmd_vel
    pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
}
//析构函数
PoseAdj::~PoseAdj()
{}
//话题ar_pose回调函数
void PoseAdj::pose_callback(const ar_track_alvar_msgs::AlvarMarkers& marker_msgs)
{
    //ROS_INFO_STREAM("marker_msgs.confidence:" << marker_msgs.confidence);
    for(int i=0;i<marker_msgs.markers.size();i++){
        if (marker_msgs.markers[i].id==track||track==1024)
        {
            /* code for True */
            //消息置信度为1，将id和pose等复制
            curr_pose.number = marker_msgs.markers[i].id;
            curr_pose.position.x = marker_msgs.markers[i].pose.pose.position.x;
            curr_pose.position.y = marker_msgs.markers[i].pose.pose.position.y;
            curr_pose.position.z = marker_msgs.markers[i].pose.pose.position.z;
            curr_pose.confidence = marker_msgs.markers[i].confidence;//marker_msgs.markers.confidence ;
            tf::Quaternion quat;
            tf::quaternionMsgToTF(marker_msgs.markers[i].pose.pose.orientation, quat);
            tf::Matrix3x3(quat).getRPY(curr_pose.roll, curr_pose.pitch, curr_pose.yaw);
            //cout << "x: " << curr_pose.position.x << "y: " << curr_pose.position.y << "z: " << curr_pose.position.z <<endl;
	    //cout << "roll :" << curr_pose.roll << endl;
	    break;
        }
    }
}

bool PoseAdj::TrackCallback(fox_msgs::track::Request &req, 
                            fox_msgs::track::Response &res)
{
    track=req.track;

    ros::Rate loop(5);
    geometry_msgs::Twist twist;
    twist.linear.x = 0;
    twist.linear.y = 0;
    twist.angular.z = 0;

    //x方向的偏差
    double x_err  = curr_pose.position.x - X_GOAL;
    double z_goal[ADJUST_TIMES] = {0};
    for(int i=0; i<ADJUST_TIMES; i++) {
        z_goal[i] = curr_pose.position.z-(curr_pose.position.z - Z_GOAL)*(i+1)/ADJUST_TIMES;
        ROS_INFO_STREAM("z_goal[" << z_goal[i] << "]");
    }

    bool track_flag = curr_pose.confidence;


	 for(int k=0; k<REPEAT_TIMES; k++) {	//repeat
            for (int i=0; i<ADJUST_TIMES; i++) {
                ROS_INFO_STREAM("adjust times[" << i << "]");
                //th偏差
                double theta_err = curr_pose.pitch - THETA_GOAL;
                //实例化类x_pid
                PID_Controller x_pid(P_Y, I_Y, D_Y);
                //实例化类y_pid
                PID_Controller z_pid(P_X, I_X, D_X);
                //实例化类theta_pid
                PID_Controller theta_pid(P_THETA, I_THETA, D_THETA);
                //更新x_err
                x_err = curr_pose.position.x - X_GOAL;
                //z_err
                double z_err = 0;
		        if(k==0) {
		            z_err= curr_pose.position.z - z_goal[i];
                }else{
                    //如果k=1 zerr
                    z_err= curr_pose.position.z - Z_GOAL;
                }
                int m=3;
                //进行pid计算
                while(((fabs(theta_err) > 0.01)||(fabs(x_err) > 0.003)||(fabs(z_err) > 0.01))) {
                    twist.angular.z = -theta_pid.PID(theta_err);
                    twist.linear.x = z_pid.PID(z_err);
                    x_err  = curr_pose.position.x - X_GOAL;
                    twist.linear.y = -x_pid.PID(x_err);
                    ROS_INFO_STREAM("x_err:" << z_err << "y_err:" << z_err << "th_err" << theta_err);
                    theta_err = curr_pose.pitch ;

                    z_err  = curr_pose.position.z - Z_GOAL;
                    //x_err  = curr_pose.position.x ;
                    track_flag = curr_pose.confidence;
                    //发布消息。
                    pub.publish(twist);
                    //延时
                    loop.sleep();
                }
                twist.linear.x = 0;
                twist.linear.y = 0;
                twist.angular.z = 0;
                pub.publish(twist);
                usleep(500*1000);
            }// end repeat
            //如果ar码位置消息不可信，则返回失败
            //if(!track_flag) {
            //    twist.linear.x = 0;
            //    twist.linear.y = 0;
            //    twist.angular.z = 0;
            //    pub.publish(twist);
            //    res.message = "failed";
            //    res.success = false;
            //    return true;
            //}
        }
        track = 1024;
        res.message = "success";
        res.success = true;
        return true;

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pose_adjust");
    //实例化类
    PoseAdj PoseAdj;
    //创建多线程接收（2）
    ros::AsyncSpinner spinner(2);
    //开始多线程接收
    spinner.start();
    ros::waitForShutdown();
}
